function Milisg_2012_10_22
clear all;
clc;
format long;
warning off all
clf(figure(1))

javaaddpath 'C:\Users\giorgos\Documents\My Dropbox\Software\PhD-SoftwareSolutions\BACnet\bacnet4J.jar';
javaaddpath 'C:\Users\giorgos\Documents\My Dropbox\Software\PhD-SoftwareSolutions\BACnet\seroUtils.jar';
javaaddpath 'C:\Users\giorgos\Documents\My Dropbox\PhD\Research\1.CodeBase\JavaCode\BACnetNetwork1\build\classes'

import java.lang.*;
import com.serotonin.bacnet4j.*; 
import com.serotonin.bacnet4j.apdu.*;  
import com.serotonin.bacnet4j.base.*; 
import com.serotonin.bacnet4j.enums.*;  
import com.serotonin.bacnet4j.event.*;  
import com.serotonin.bacnet4j.exception.*;  
import com.serotonin.bacnet4j.npdu.*;  
import com.serotonin.bacnet4j.npdu.ip.*;  
import com.serotonin.bacnet4j.obj.*;  
import com.serotonin.bacnet4j.service.*;  
import com.serotonin.bacnet4j.service.acknowledgement.*;  
import com.serotonin.bacnet4j.service.confirmed.*;  
import com.serotonin.bacnet4j.service.unconfirmed.*;  
import com.serotonin.bacnet4j.test.*;  
import com.serotonin.bacnet4j.type.*;  
import com.serotonin.bacnet4j.type.constructed.*;  
import com.serotonin.bacnet4j.type.enumerated.*;  
import com.serotonin.bacnet4j.type.error.*;  
import com.serotonin.bacnet4j.type.eventParameter.*;  
import com.serotonin.bacnet4j.type.notificationParameters.*;  
import com.serotonin.bacnet4j.type.primitive.*;  
import com.serotonin.bacnet4j.util.*;  
import bacnetnetwork1.*;

%parameters of plant (three-rooms system)
n=3; % number of states

vAir=0.104; % m/s
c1=10.45 - vAir+10*sqrt(vAir); %heat transfer coefficient of air (W/m^2.K) - c = 10.45 ? v + 10?v , with normal v air = 0.104 m/s. For v=20 m/s, the coefficient becomes 35.17
c2=0.5/0.2; % heat transfer coefficient of brick =thermal conductivity (e.g. 70 W/m.C) / thickness (e.g. (50 mm) - (W/m^2.C^0). %we assume walls of thickness 20cm=0.2m and material brick of 3Kg with thermal
%conductivity 0.5
ro=1.29; %density of air (Kg/m3)
cp=1005; %specific heat capacity of air (J/kg.K)
Adoor=2.0; % Area of door (m^2)
Awindow=0.5; % Area of window (m^2)
Awall=9; % Floor and roof area = 3x3
%Awe=3*2.5; % Area of east-west walls
%Asn=4*2.5; % Area of north-south walls
V=3*3*3; % volume of room (m3)
H=cp*ro*V; % Heat capacity of air in room (J/C)
uMax=2000;
stepSize=100;
uSteps=[-uMax:stepSize:uMax]; % steps of inflow heat transfer by electric heater (W)
TaMax=22; %oC
TaMin=8; %oC
TaMag=TaMax-TaMin; % magnidute of air temperature variations
TaMean=(TaMax+TaMin)/2; % mean value of air temperature in a day

%x_dot=[-Cf*4*Aw -Cf*Aw 0;Cf*Aw -Cf*Aw-Cf*4*Aw-Cf*Aw +Cf*Aw;0 Cf*Aw -Cf*Aw-Cf*4*Aw]*[x1 x2 x3]'+[1/H 0 0]*U + [+Cf*4*Aw*Ta +Cf*3*Aw +Cf*4*Aw];
A=1/H.*[-c1*Adoor c1*Adoor 0;c1*Adoor -c1*Adoor-c1*Adoor c1*Adoor;0 c1*Adoor -c1*Adoor];
B=1/H.*[1; 0; 0];
%D=1/H.*[4*c2*Awall+c1*Awindow; 3*c2*Awall+c1*Awindow; 4*c2*Awall+c1*Awindow];%*Ta
C=[1 0 0;0 1 0;1 0 0];

%Definition of scenarios for execution - %other initialization and
%configuration parameters

mediation=3;
r=25; %desired/reference temperature for room in Celsius
superTitle={'Yes changes - Yes mediation'};
simulationTime=50;
simulateHourTime=60;
Ts=5/60; % sampling time 5 minutes
pricePerKwh=0.5; % euro per KWh
k=1; %discrete step counter

%%%%%%%%%%%%%%%%%%% BACnet-Network %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bacnetNetworkPort=2029;

bacnetNetworkAddress='127.0.0.255';
bacnetNetworkNumber=1;
bacnetNetwork=BacNetNetwork(bacnetNetworkNumber,bacnetNetworkAddress);

% Sensors
sensor1Address='127.0.0.1';
sensor2Address='127.0.0.2';
sensor3Address='127.0.0.3';
sensor4Address='127.0.0.4';
sensor11Address='127.0.0.11';
sensor12Address='127.0.0.12';
sensor21Address='127.0.0.21';

sensor1=Sensor(1, bacnetNetworkAddress, sensor1Address, bacnetNetworkPort, 'Sensor 1', EngineeringUnits.degreesCelsius);
sensor1Annotation=SensorAnnotation(1,'temperature', 'celsius', 5, 'Room 1', 0);
sensor1.setOutOfService(0);
sensor2=Sensor(2, bacnetNetworkAddress, sensor2Address, bacnetNetworkPort, 'Sensor 2', EngineeringUnits.degreesCelsius);
sensor2Annotation=SensorAnnotation(2,'temperature', 'celsius', 5, 'Room 2', 0);
sensor2.setOutOfService(0);
sensor3=Sensor(3, bacnetNetworkAddress, sensor3Address, bacnetNetworkPort, 'Sensor 3', EngineeringUnits.degreesCelsius);
sensor3Annotation=SensorAnnotation(3,'temperature', 'celsius', 5, 'Room 3', 0);
sensor3.setOutOfService(0);
sensor4=Sensor(4, bacnetNetworkAddress, sensor4Address, bacnetNetworkPort, 'Sensor 4', EngineeringUnits.degreesFahrenheit);
sensor4Annotation=SensorAnnotation(4,'temperature', 'fahrenheit', 2, 'Room 3', 0);
sensor4.setOutOfService(1);
sensor11=Sensor(11, bacnetNetworkAddress, sensor11Address, bacnetNetworkPort, 'Sensor 11', EngineeringUnits.degreesCelsius);
sensor11Annotation=SensorAnnotation(11,'temperature', 'celsius', 1, 'Room 2', 0);
sensor11.setOutOfService(1);
sensor12=Sensor(12, bacnetNetworkAddress, sensor12Address, bacnetNetworkPort, 'Sensor 12', EngineeringUnits.degreesCelsius);
sensor12Annotation=SensorAnnotation(12,'temperature', 'celsius', 2, 'Room 3', 0);
sensor12.setOutOfService(1);
sensor21=Sensor(21, bacnetNetworkAddress, sensor21Address, bacnetNetworkPort, 'Sensor 21', EngineeringUnits.degreesCelsius);
sensor21Annotation=SensorAnnotation(21,'temperature', 'celsius', 2, 'Ambient', 0);
sensor21.setOutOfService(1);

% Actuators - ElectricRadiators
eRadiator1Address='127.0.0.101';
eRadiator1=ElectricRadiator(101, bacnetNetworkAddress, eRadiator1Address, bacnetNetworkPort, 'Elecrtic Radiator 1', EngineeringUnits.watts);

% Controller
controller1Address='127.0.0.201';
controller1=Controller(201, bacnetNetworkAddress, controller1Address, bacnetNetworkPort, 'Controller 1', EngineeringUnits.watts);

% SSM device
ssm1Address='127.0.0.202';
ssm1=SSM(202, bacnetNetworkAddress, ssm1Address, bacnetNetworkPort, 'SSM 1');

communicate=Communicate();

%controlled system parameters + controller design parameters
am=4.0;
bm=am;
amd=4;
bmd=4;
a=A(1,1);
b=B(1);
objId1=controller1.getObject('Control parameter 1 value').getId();
objId2=controller1.getObject('Field parameter 1 value').getId();
objId3=controller1.getObject('Field parameter 2 value').getId();
prId1=PropertyIdentifier.presentValue;
communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId1, prId1, am);
Thread.sleep(1000);
communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId2, prId1, a);
Thread.sleep(1000);
communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId3, prId1, b);
Thread.sleep(1000);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%definition of time-related information
tstart = 1;
tend = simulationTime*simulateHourTime;
steps = 1; % steps of 1 minute
timespan=(tstart:steps:tend);
thours=timespan./simulateHourTime;

%modelling of outside temperature, as well as, bounds for outside
%temperature and Room 2 temperature
Ta=modelOutsideTemperature(thours, TaMean, TaMag);
[TaUpperBound TaLowerBound] = modelBoundsOutsideTemperature(Ta);
[T2UpperBound T2LowerBound] = modelBoundsRoom2Temperature(Ta);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Beginning of scenario
for iter=tstart:steps:tend    
    currentTime=thours(iter);

    %%%%%%%%%%%%%%%%%%%Implement scenarios of sensors%%%%%%%%%%%%%%%
    if (currentTime >= 1)
%         objId=sensor11.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor11AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(false), UnsignedInteger(5)));
        sensor11.setOutOfService(0);
    elseif (currentTime >= 2)
%         objId=sensor11.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor11AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(true), UnsignedInteger(5)));
        sensor11.setOutOfService(1);
    elseif (currentTime >= 5)
%         objId=sensor3.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor3AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(true), UnsignedInteger(5)));
        sensor3.setOutOfService(1);
    elseif (currentTime >= 6)
%         objId=sensor12.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor12AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(false), UnsignedInteger(5)));
        sensor12.setOutOfService(0);
    elseif (currentTime >= 7)
%         objId=sensor12.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor12AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(true), UnsignedInteger(5)));
        sensor12.setOutOfService(1);
%         objId=sensor11.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         objId=sensor21.getObject('Sensor output').getId();
%         ssm1.send(Address(sensor21AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(false), UnsignedInteger(5)));
        sensor21.setOutOfService(0);
    elseif (currentTime >= 17)
        sensor11Annotation.setLocation('Room 3');
%         objId=sensor11.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor11AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(false), UnsignedInteger(5)));
        sensor11.setOutOfService(0);
    elseif (currentTime >= 19)
%         objId=sensor11.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor11AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(true), UnsignedInteger(5)));
        sensor11.setOutOfService(1);
    elseif (currentTime >= 22)
%         objId=sensor21.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor21AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(true), UnsignedInteger(5)));
        sensor21.setOutOfService(1);
    elseif (currentTime >= 29)
%         objId=sensor4.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.outOfService;
%         ssm1.send(Address(sensor4AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Boolean(false), UnsignedInteger(5)));
        sensor4.setOutOfService(0);
    end 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % initializations during first step of scenario
    if (iter==1)
        %initialization of dynamic variables and integration/simulation
        xIni=[18 18 18]';
        ydIni=[r r r]';
        yddotIni=-amd*eye(n)*ydIni+bmd*eye(n)*[r r r]';

        dl1=A(1,2)*T2LowerBound(1);
        dl2=D(1)*TaLowerBound(1);
        dl=dl1+dl2;
        du1=A(1,2)*T2UpperBound(1);
        du2=D(1)*TaUpperBound(1);
        du=du1+du2;
        
%         objId=sensor1.getObject('Sensor output').getId();
%         prId=PropertyIdentifier.presentValue;
%         ssm1.send(Address(sensor1AddressBytes,bacnetNetworkPort), null, 35, Segmentation.segmentedBoth, WritePropertyRequest(objId, prId, null, Real(xIni(1)), UnsignedInteger(5)));
        sensor1.setSensorValue(xIni(1)); 
        sensor2.setSensorValue(xIni(2));
        sensor3.setSensorValue(xIni(3));
        ssm1.setOutputValue(1, xIni(1));
        ssm1.setOutputValue(2, xIni(2));
        ssm1.setOutputValue(3, xIni(3));
        
        objId=controller1.getObject('Reference rate value').getId();
        prId=PropertyIdentifier.presentValue;
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, yddotIni(1));
        objId=controller1.getObject('Reference value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, ydIni(1));
        objId=controller1.getObject('Disturbance 1 lower bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, dl1);
        objId=controller1.getObject('Disturbance 2 lower bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, dl2);
        objId=controller1.getObject('Disturbance 1 upper bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, du1);
        objId=controller1.getObject('Disturbance 2 upper bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, du2);

        
        adaptUpperTaIni=1;
        adaptLowerTaIni=1;
        adaptUpperT2Ini=1;
        adaptLowerT2Ini=1;
        adaptBoundsIni=[adaptUpperTaIni adaptLowerTaIni adaptUpperT2Ini adaptLowerT2Ini];
        uIni=produceControllerOutput(communicate,controller1, ssm1, ssm1Address, eRadiator1, eRadiator1Address, bacnetNetworkPort, adaptBoundsIni,uSteps);
        
        %initialization of progress bar
        h = waitbar(0,'Initializing waitbar...','Name','Progress...');
        stepsint=100;
        stateIni=[xIni' ydIni' uIni adaptBoundsIni];
        MassMatrix=diag([ones(1,n) ones(1,n) ones(1,1) ones(1,4)]);
        options =odeset('Mass',MassMatrix,'Reltol',0.1,'AbsTol',0.1,'Stats','off','MassSingular','no');            
        
        yfinal=C*xIni;
        TaCurrent=TaMean;
        uref=uIni;        
    else           
        %tasks at subsequent steps of scenario    
        
        % upper and lower bounds of system disturbance
        dl1=A(1,2)*T2LowerBound(iter);
        dl2=D(1)*TaLowerBound(iter);
        dl=dl1+dl2;
        du1=A(1,2)*T2UpperBound(iter);
        du2=D(1)*TaUpperBound(iter);
        du=du1+du2;
        
        stateIni=[X(:,iter-1)' Yd(:,iter-1)' U(iter-1) AdaptBounds(:,iter-1)'];        
    end
    
    %parameters for integration
    tspan = linspace(iter-1,iter,stepsint);
    varargin={n,A,B,D,TaCurrent,amd,bmd,r.*ones(n,1),uref,du1,du2,dl1,dl2};
    [junk_t,solution] = ode45(@myodefun, tspan, stateIni,options,varargin);

    % processing of integration outcome
    [s junk]=size(junk_t);
    X(:,iter)=(solution(s,1:n))';
    Yd(:,iter)=(solution(s,n+1:2*n))';
    U(iter)=(solution(s,2*n+1))';
    AdaptBounds(:,iter)=(solution(s,2*n+2:2*n+5));
    
    % dream measurement
    y=C*X(:,iter);
    yddot=-amd*eye(n)*Yd(:,iter)+bmd*eye(n)*(r.*ones(n,1));
 
    % recording of measurements from available sensors
    recordMeasurement(y,sensor1,sensor1Annotation,currentTime);
    recordMeasurement(y,sensor2,sensor2Annotation,currentTime);
    recordMeasurement(y,sensor3,sensor3Annotation,currentTime);
    recordMeasurement(y,sensor4,sensor4Annotation,currentTime);
    recordMeasurement(y,sensor11,sensor11Annotation,currentTime);
    recordMeasurement(y,sensor12,sensor12Annotation,currentTime);
    recordMeasurement(y,sensor21,sensor21Annotation,currentTime);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %retrieval of measurements and processing to produce the control
    %feedback vector    
    samplingTime=k*Ts;
    if (currentTime >= samplingTime)
        for ii=1:n+1
            y2(ii,1)=0;
            y2(ii,2)=0;
        end
    
        [value1 loc1 when1 accur1]=retrieveMeasurement(communicate,sensor1,sensor1Annotation,sensor1Address,ssm1,bacnetNetworkPort);        
        totalWeight1=weightTime(currentTime, when1)*weightAccuracy(accur1);
        y2(loc1,1)=y2(loc1,1)+value1*totalWeight1;
        y2(loc1,2)=y2(loc1,2)+totalWeight1;
        
        [value2 loc2 when2 accur2]=retrieveMeasurement(communicate,sensor2,sensor2Annotation,sensor1Address,ssm1,bacnetNetworkPort);
        totalWeight2=weightTime(currentTime, when2)*weightAccuracy(accur2);
        y2(loc2,1)=y2(loc2,1)+value2*totalWeight2;
        y2(loc2,2)=y2(loc2,2)+totalWeight2;
        
        [value3 loc3 when3 accur3]=retrieveMeasurement(communicate,sensor3,sensor3Annotation,sensor1Address,ssm1,bacnetNetworkPort);
        totalWeight3=weightTime(currentTime, when3)*weightAccuracy(accur3);
        y2(loc3,1)=y2(loc3,1)+value3*totalWeight3;
        y2(loc3,2)=y2(loc3,2)+totalWeight3;
        
        [value4 loc4 when4 accur4]=retrieveMeasurement(communicate,sensor4,sensor4Annotation,sensor1Address,ssm1,bacnetNetworkPort);
        totalWeight4=weightTime(currentTime, when4)*weightAccuracy(accur4);
        y2(loc4,1)=y2(loc4,1)+value4*totalWeight4;
        y2(loc4,2)=y2(loc4,2)+totalWeight4;
        
        [value11 loc11 when11 accur11]=retrieveMeasurement(communicate,sensor11,sensor11Annotation,sensor1Address,ssm1,bacnetNetworkPort);
        totalWeight11=weightTime(currentTime, when11)*weightAccuracy(accur11);
        y2(loc11,1)=y2(loc11,1)+value11*totalWeight11;
        y2(loc11,2)=y2(loc11,2)+totalWeight11;
        
        [value12 loc12 when12 accur12]=retrieveMeasurement(communicate,sensor12,sensor12Annotation,sensor1Address,ssm1,bacnetNetworkPort);
        totalWeight12=weightTime(currentTime, when12)*weightAccuracy(accur12);
        y2(loc12,1)=y2(loc12,1)+value12*totalWeight12;
        y2(loc12,2)=y2(loc12,2)+totalWeight12;
        
        [value21 loc21 when21 accur21]=retrieveMeasurement(communicate,sensor21,sensor21Annotation,sensor1Address,ssm1,bacnetNetworkPort);   
        totalWeight21=weightTime(currentTime, when21)*weightAccuracy(accur21);
        y2(loc21,1)=y2(loc21,1)+value21*totalWeight21;
        y2(loc21,2)=y2(loc21,2)+totalWeight21;
               
        for ii=1:n+1            
            yfinal(ii)=y2(ii,1)/y2(ii,2);
        end
        
        ssm1.getObject('SSM output 1').setProperty(PropertyIdentifier.presentValue, Real(yfinal(1)));
        
        objId=controller1.getObject('Reference rate value').getId();
        prId=PropertyIdentifier.presentValue;
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, yddot(1));
        objId=controller1.getObject('Reference value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, Yd(1,iter));
        objId=controller1.getObject('Disturbance 1 lower bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, dl1);
        objId=controller1.getObject('Disturbance 2 lower bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, dl2);
        objId=controller1.getObject('Disturbance 1 upper bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, du1);
        objId=controller1.getObject('Disturbance 2 upper bound value').getId();
        communicate.communicateWriteReal(ssm1,controller1Address,bacnetNetworkPort,objId, prId, du2);       
        
        uref=produceControllerOutput(communicate,controller1, ssm1, ssm1Address, eRadiator1, eRadiator1Address, bacnetNetworkPort,AdaptBounds(:,iter),uSteps);
        
        k=k+1;
    end        
    
    %calling of progress bar
    progressBar(thours(iter),simulationTime,h);
end %end of iterations of scenario    
    
%closing progress bar
close(h); 
%stop devices
sensor1.terminate();
sensor2.terminate();
sensor3.terminate();
sensor4.terminate();
sensor11.terminate();
sensor12.terminate();
sensor21.terminate();
eRadiator1.terminate();
controller1.terminate();
ssm1.terminate();
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%calculation of total cost for heating
EconomicCost=calcEconomicCost(pricePerKwh, U,timespan)
FunctionalCost=calcFunctionalCost(Yd, X)    
TotalCost=0.3*EconomicCost+0.7*FunctionalCost
%%%%%%%%%%%%%%%%%%%

%plots of solution and other outputs
f=figure(1);

fontname='Times-Roman';
fontsize=12;

subplot(2,1,1);
h1=plot(thours,X(1,:),'-r',thours,X(2,:),'-b',thours,X(3,:),'-g');
set(gca,'FontSize',fontsize,'FontName',fontname);
grid on;
axis([0 60 0 30]);
title1=title('System State Vs Time');
xlabel1=xlabel('Time (Hours)');
ylabel1=ylabel('Temperature (Fahrenheit)');
leg1=legend('$T_1$', '$T_2$','$T_3$');
set(h1,'LineWidth',2)
set(leg1,'interpreter','latex','FontSize',fontsize,'FontName',fontname,'Layer','top');
set(title1,'FontSize',fontsize,'FontName',fontname,'FontWeight','bold');
set(xlabel1,'FontSize',fontsize,'FontName',fontname);
set(ylabel1,'FontSize',fontsize,'FontName',fontname);

%Control input Vs Time
subplot(2,1,2);
h1=plot(thours,U(:),'-r');
set(gca,'FontSize',fontsize,'FontName',fontname);
grid on;
axis([0 60 -2500 2500]);
title3=title('Control input Vs Time');
xlabel3=xlabel('Time (Hours)');
ylabel3=ylabel('Control input (Watts)');
leg3=legend('$u_1$');
set(h1,'LineWidth',2);
set(leg3,'interpreter','latex','FontSize',fontsize,'FontName',fontname,'Layer','top'); 
set(title3,'FontSize',fontsize,'FontName',fontname,'FontWeight','bold');
set(xlabel3,'FontSize',fontsize,'FontName',fontname);
set(ylabel3,'FontSize',fontsize,'FontName',fontname);  

tbh = annotation(f,'textbox',[0.02 0.96 0.9 0.04]);
titletextw = textwrap(superTitle,180);
set(tbh,'EdgeColor','none','Interpreter','none','string',titletextw,'FontSize',20,'FontWeight','bold','HorizontalAlignment','center');   

%figure(2);
%plot(thours,AdaptBounds(1,:),thours,AdaptBounds(2,:),thours,AdaptBounds(3,:),thours,AdaptBounds(4,:))
%grid;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function performing the integration of the differential equations, given
% the current state.
function [dsdt] = myodefun(t,state,varargin)

n=varargin{1}{1};
A=varargin{1}{2};
B=varargin{1}{3};
D=varargin{1}{4};
Ta=varargin{1}{5};
amd=varargin{1}{6};
bmd=varargin{1}{7};
r=varargin{1}{8};
uref=varargin{1}{9};
du1=varargin{1}{10};
du2=varargin{1}{11};
dl1=varargin{1}{12};
dl2=varargin{1}{13};


%logging of the states
x=state(1:n);
yd=state(n+1:2*n);
u=state(2*n+1);
adaptBounds=state(2*n+2:2*n+5);

error=x(1)-yd(1);

xdot=A*x+B*u+D*Ta;
yddot=-amd*eye(n)*yd+bmd*eye(n)*r;
udot=-8*u+8*uref;

gammaUpper=2;
gammaLower=2;
if (error >=0)
    adaptBoundsDot(1)=gammaUpper*error*du1;
    adaptBoundsDot(3)=gammaUpper*error*du2;
    adaptBoundsDot(2)=0;
    adaptBoundsDot(4)=0;
else
    adaptBoundsDot(1)=0;
    adaptBoundsDot(3)=0;
    adaptBoundsDot(2)=gammaLower*error*dl1;
    adaptBoundsDot(4)=gammaLower*error*dl2;
end

dsdt=zeros(size(state));
dsdt(1:n)=xdot; 
dsdt(n+1:2*n)=yddot;
dsdt(2*n+1)=udot; 
dsdt(2*n+2:2*n+5)=adaptBoundsDot;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f=SEMfunction1(x)
    f=(x-32)/1.8;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function progressBar(thours,simulationTime,h)
 
perc = thours*100/simulationTime;
waitbar(perc/100,h,sprintf('%12.4g%% along...',perc));        
end
% --------------------------------------------------------------------------
function EconomicCost = calcEconomicCost(pricePerKwh, U, timespan)
[m s2]=size(U);
for i=1:m
    cost(i)=(abs(U(i,1))/1000)*timespan(1)*pricePerKwh;
    for j=2:s2
        cost(i)=cost(i)+(abs(U(i,j))/1000)*(timespan(j)-timespan(j-1))*pricePerKwh;
    end        
end

EconomicCost=0;
for i=1:m
    EconomicCost=EconomicCost+cost(i);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FunctionalCost = calcFunctionalCost(Yd, X)
[n s]=size(X);
for i=1:s
    Xr(:,i)=(X(:,i)-Yd(:,i)).^2;
end

for i=1:n
    cost(i)=0;
    for j=1:s
        cost(i)=cost(i)+Xr(i,j);
    end        
end
FunctionalCost=0;
for i=1:n
    FunctionalCost=FunctionalCost+cost(i);
end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function uProcessed = processControlInput(u, uSteps)
    tmp = abs(u-uSteps);
    [idx idx] = min(tmp); %index of closest value
    closest = uSteps(idx); %closest value
    uProcessed=closest;    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Ta = modelOutsideTemperature(thours,TaMean,TaMag)
    for ii=1:length(thours)
        tcurrent=mod(thours(ii),24);
        if (tcurrent==0)
            tcurrent=24;
        end
        omega=2*pi*(tcurrent-1)/24;
        Ta(ii)=TaMean+TaMag*(0.4632*cos(omega-3.805)+0.0984*cos(2*omega-0.360)+0.0168*cos(3*omega-0.822)+0.0138*cos(4*omega-3.513));   
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [TaUpperBound TaLowerBound] = modelBoundsOutsideTemperature(Ta)
    for ii=1:length(Ta)
        TaUpperBound(ii)=Ta(ii)+2;
        TaLowerBound(ii)=Ta(ii)-2;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [T2UpperBound T2LowerBound] = modelBoundsRoom2Temperature(Ta)
    for ii=1:length(Ta)
        T2UpperBound(ii)=Ta(ii)+4;
        T2LowerBound(ii)=Ta(ii);
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function controllerOutput=produceControllerOutput(communicate,controller,ssm,ssmAddress, eRadiator, eRadiatorAddress,bacnetNetworkPort, adaptBounds, uSteps)
    import com.serotonin.bacnet4j.type.enumerated.*;      
    epsilon=0.001;
    objId=ssm.getObject('SSM output 1').getId();
    prId=PropertyIdentifier.presentValue;
    measurement=communicate.communicateRead(controller,ssmAddress,bacnetNetworkPort,objId, prId).floatValue();
    
    reference=controller.getReferenceValue().floatValue();
    disturbance1LowerBoundValue=controller.getDisturbance1LowerBoundValue().floatValue();    
    disturbance2LowerBoundValue=controller.getDisturbance2LowerBoundValue().floatValue();    
    disturbance1UpperBoundValue=controller.getDisturbance1UpperBoundValue().floatValue();    
    disturbance2UpperBoundValue=controller.getDisturbance2UpperBoundValue().floatValue();   
    controlParameter1Value=controller.getControlParameter1Value().floatValue();    
    fieldParameter1Value=controller.getFieldParameter1Value().floatValue();    
    fieldParameter2Value=controller.getFieldParameter2Value().floatValue();    
    referenceRateValue=controller.getReferenceRateValue().floatValue();   
    
    error=(measurement-reference); 
    if (error>epsilon) && (disturbance1UpperBoundValue>0)
        vf1=adaptBounds(1)*disturbance1UpperBoundValue;
        vf2=adaptBounds(3)*disturbance2UpperBoundValue;
    elseif (error<-epsilon) && (disturbance1LowerBoundValue<0)
        vf1=adaptBounds(2)*disturbance1LowerBoundValue;
        vf2=adaptBounds(4)*disturbance2LowerBoundValue;
    elseif (abs(error)<=epsilon)
        vf1=(1/(2*epsilon))*((epsilon-error)*adaptBounds(2)*disturbance1LowerBoundValue+(epsilon+error)*adaptBounds(1)*disturbance1UpperBoundValue);
        vf2=(1/(2*epsilon))*((epsilon-error)*adaptBounds(4)*disturbance2LowerBoundValue+(epsilon+error)*adaptBounds(3)*disturbance2UpperBoundValue);
    else
        vf1=0;
        vf2=0;
    end
        
    controllerOutput=processControlInput((-controlParameter1Value*error-fieldParameter1Value*measurement+referenceRateValue-vf1-vf2)/fieldParameter2Value,uSteps);
    controller.setControllerOutput(controllerOutput);    
    objId=eRadiator.getObject('Actuator output').getId();
    prId=PropertyIdentifier.presentValue;
    communicate.communicateWriteReal(controller,eRadiatorAddress,bacnetNetworkPort,objId, prId, controllerOutput);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function loc=where(location)
    if strcmp(location,'Room 1')
            loc=1;
    elseif strcmp(location,'Room 2')
            loc=2;
    elseif strcmp(location,'Room 3')
            loc=3;
    elseif strcmp(location,'Ambient')
            loc=4;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function recordMeasurement(y,sensor,sensorAnnotation,currentTime)
    sensorOutOfSercice=sensor.getOutOfService();
    if (sensorOutOfSercice=='false')     
        aa=y(where(sensorAnnotation.getLocation()))*(1-sensorAnnotation.getAccuracy()/100);
        bb=y(where(sensorAnnotation.getLocation()))*(1+sensorAnnotation.getAccuracy()/100);
        tempValue=aa + (bb-aa)*rand(1,1);
        if strcmp(sensorAnnotation.getUnits(),'celsius')
            sensor.setSensorValue(tempValue);
        elseif strcmp(sensorAnnotation.getUnits(),'fahrenheit')
            sensor.setSensorValue(1.8*tempValue+32);
        end
        sensorAnnotation.setLastMeasurementTime(currentTime);
    end    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [value loc when accur] = retrieveMeasurement(communicate,sensor,sensorAnnotation,sensorAddress,ssm, bacnetNetworkPort)
    import com.serotonin.bacnet4j.type.enumerated.*;      
    objId=sensor.getObject('Sensor output').getId();
    prId=PropertyIdentifier.presentValue; 
    result=communicate.communicateRead(ssm,sensorAddress,bacnetNetworkPort,objId, prId).floatValue();
    if strcmp(sensorAnnotation.getUnits(),'celsius')
        value=result;
    elseif strcmp(sensorAnnotation.getUnits(),'fahrenheit')
        value=SEMfunction1(result);
    end
    loc=where(sensorAnnotation.getLocation());
    when=sensorAnnotation.getLastMeasurementTime();
    accur=sensorAnnotation.getAccuracy();
    
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wt=weightTime(currentTime, when)
    rejectTime=5/60;
    if (currentTime-when > rejectTime)
        wt=0;
    else
        wt=((rejectTime-(currentTime-when)))^2;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wa=weightAccuracy(accur)
    wa=1/accur^2;
end